#include "MicroBit.h"

#define DEBUG 0

#define ON_LISTEN(id, v, num)                                                                      \
    uBit.messageBus.listen(                                                                        \
        id, v, [](MicroBitEvent) -> void { uartWrite(num, v); }, MESSAGE_BUS_LISTENER_REENTRANT)

#define ON_GESTURE(v) ON_LISTEN(MICROBIT_ID_GESTURE, v, GestureEvent)
#define ON_LOGO(v) ON_LISTEN(MICROBIT_ID_LOGO, v, LogoEvent)

const char CM_DEL = 0xFF;
const char LightLevel = 0x01;
const char SoundLevel = 0x02;
const char ServoWrite = 0x03;
const char IsGesture = 0x04;
const char CompassHeading = 0x05;
const char PlayTone = 0x06;
const char SetVolume = 0x07;
const char SerovoSetPulse = 0x08;
const char EnablePinTouchMode = 0x09;
const char GestureEvent = 0x6f;
const char P0Event = 0x70;
const char P1Event = 0x71;
const char P2Event = 0x72;
const char LogoEvent = 0x7F;

const char happyBits[] = {
    0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 1, 1, 1, 0,
};

const char sadBits[] = {
    0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 1, 1, 1, 0, 1, 0, 0, 0, 1,
};

const char startFrame[] = {
    1, 0, 0, 0, 1, //
    1, 1, 0, 1, 1, //
    1, 0, 1, 0, 1, //
    1, 0, 0, 0, 1, //
    1, 0, 0, 0, 1, //
};

char frame1[] = {
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
};

char frame2[] = {
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
};

char frame3[] = {
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
    0, 0, 0, 0, 0, //
};

const char endFrame[] = {
    1, 0, 0, 1, 0, //
    1, 0, 0, 1, 0, //
    1, 1, 1, 1, 0, //
    0, 0, 0, 1, 0, //
    0, 0, 0, 1, 0, //
};

bool connected = false;
bool waitingPlayTone = false;

int checkCount = 0;

MicroBit uBit;
MicroBitUARTService *uart;

void showImage(char *image)
{
    uBit.display.stopAnimation();
    for (int y = 0; y < 5; y++)
        for (int x = 0; x < 5; x++)
            uBit.display.image.setPixelValue(x, y, image[y * 5 + x] ? 255 : 0);
}

void uartWrite(uint8_t event, uint16_t value)
{
    if (!connected)
        return;

    uint8_t low = 0x7F & value;
    uint8_t high = 0x7F & (value >> 7);

    uint8_t len = 4;
    ManagedBuffer buffer(len);
    buffer.setByte(0, event);
    buffer.setByte(1, low);
    buffer.setByte(2, high);
    buffer.setByte(3, CM_DEL);
    uart->send(buffer.getBytes(), len);

#if DEBUG
    uBit.serial.send(buffer.getBytes(), len);
#endif
}

char checkSkipStop()
{
    checkCount++;

    if (checkCount > 4)
    {
        checkCount = 0;
        waitingPlayTone = false;
        return 1; // not skip
    }

    if (waitingPlayTone)
    {
        checkCount = 0;
        waitingPlayTone = false;
        return 0; // skip
    }

    return 2; // continue check
}

void stopTone()
{
    while (true)
    {
        char checkResult = checkSkipStop();

        if (checkResult == 0)
        {

#if DEBUG
            uBit.serial.sendChar((char)0xf0);
#endif
            break;
        }
        else if (checkResult == 1)
        {

#if DEBUG
            uBit.serial.sendChar((char)0xf1);
#endif

            uBit.audio.virtualOutputPin.setAnalogValue(0);
            break;
        }
        else
        {
            fiber_sleep(50);
        }
    }
}

void playTone(int value)
{
    if (value <= 0)
    {
        stopTone();
        return;
    }

    if (checkCount > 0)
    {
        waitingPlayTone = true;
    }

    // setAnalogValueNoUpdate overide
    uBit.audio.virtualOutputPin.setAnalogValue(1 << (uBit.audio.getVolume() >> 5));
    uBit.audio.virtualOutputPin.setAnalogPeriodUs(1000000 / value);
}

void onDelim(MicroBitEvent)
{
    int size = uart->rxBufferedSize();

    if (size <= 0)
        return;

    ManagedBuffer buf(size);

    uart->read(buf.getBytes(), size);

    uint8_t i = 0;
    uint8_t lastIndex = 0;

    int value = 0;

    uint8_t low = 0;
    uint8_t high = 0;

    while (i < buf.length())
    {
        if (buf.getByte(i) == CM_DEL)
        {
            ManagedBuffer message = buf.slice(lastIndex, i - lastIndex + 1);

#if DEBUG
            uBit.serial.send(message.getBytes(), message.length());
#endif
            lastIndex = i + 1;

            switch (message.getByte(0))
            {
            case LightLevel:
                uartWrite(LightLevel, uBit.display.readLightLevel());
                break;

            case SoundLevel:
                uBit.sleep(100);
                uartWrite(SoundLevel, uBit.audio.levelSPL->getValue() - 80);
                break;

            case ServoWrite:
                low = message.getByte(2);
                high = message.getByte(3);
                value = (high << 7) | low;

                switch (message.getByte(1))
                {
                case MICROBIT_ID_IO_P0:
                    uBit.io.P0.setServoValue(value);
                    break;
                case MICROBIT_ID_IO_P1:
                    uBit.io.P1.setServoValue(value);
                    break;
                case MICROBIT_ID_IO_P2:
                    uBit.io.P2.setServoValue(value);
                    break;
                default:
                    break;
                }
                break;

            case SerovoSetPulse:
                low = message.getByte(2);
                high = message.getByte(3);
                value = (high << 7) | low;

                switch (message.getByte(1))
                {
                case MICROBIT_ID_IO_P0:
                    uBit.io.P0.setServoPulseUs(value);
                    break;
                case MICROBIT_ID_IO_P1:
                    uBit.io.P1.setServoPulseUs(value);
                    break;
                case MICROBIT_ID_IO_P2:
                    uBit.io.P2.setServoPulseUs(value);
                    break;
                default:
                    break;
                }
                break;

            case IsGesture:
                low = message.getByte(1);
                uartWrite(IsGesture, low == uBit.accelerometer.getGesture() ? 0x01 : 0x00);
                break;

            case CompassHeading:
                uartWrite(CompassHeading, uBit.compass.heading());
                break;

            case PlayTone:
                low = message.getByte(1);
                high = message.getByte(2);
                value = (high << 7) | low;
                playTone(value);
                break;

            case SetVolume:
                low = message.getByte(1);
                high = message.getByte(2);
                value = (high << 7) | low;
                uBit.audio.setVolume(value);
                break;

            case EnablePinTouchMode:
                value = message.getByte(2);
                switch (message.getByte(1))
                {
                case MICROBIT_ID_IO_P0:
                    uBit.io.P0.isTouched(value == 0x01 ? TouchMode::Capacitative
                                                       : TouchMode::Resistive);
                    break;
                case MICROBIT_ID_IO_P1:
                    uBit.io.P1.isTouched(value == 0x01 ? TouchMode::Capacitative
                                                       : TouchMode::Resistive);
                    break;
                case MICROBIT_ID_IO_P2:
                    uBit.io.P2.isTouched(value == 0x01 ? TouchMode::Capacitative
                                                       : TouchMode::Resistive);
                    break;
                default:
                    break;
                }
                break;

            default:
                break;
            }
        }

        i++;
    }
}

void onBLEConnect(MicroBitEvent)
{
    connected = true;
    showImage((char *)happyBits);
}

void onBLEDisconnect(MicroBitEvent)
{
    connected = false;
    showImage((char *)sadBits);
    playTone(0);
}

void genAnimationFrames()
{
    for (int y = 0; y < 5; y++)
    {
        frame1[y * 5 + 1] = startFrame[y * 5 + 0];
        frame1[y * 5 + 2] = startFrame[y * 5 + 2];
        frame1[y * 5 + 3] = startFrame[y * 5 + 4];

        frame2[y * 5 + 2] = startFrame[y * 5 + 0] | startFrame[y * 5 + 2] | startFrame[y * 5 + 4];

        frame3[y * 5 + 1] = endFrame[y * 5 + 0];
        frame3[y * 5 + 2] = endFrame[y * 5 + 2];
        frame3[y * 5 + 3] = endFrame[y * 5 + 4];
    }
}

void bootAnimation()
{
    playTone(880);
    uBit.display.image.setPixelValue(1, 4, 255);
    uBit.display.image.setPixelValue(3, 4, 255);
    fiber_sleep(75);
    uBit.display.image.setPixelValue(1, 4, 0);
    uBit.display.image.setPixelValue(3, 4, 0);
    uBit.display.image.setPixelValue(0, 4, 255);
    uBit.display.image.setPixelValue(1, 3, 255);
    uBit.display.image.setPixelValue(2, 3, 255);
    uBit.display.image.setPixelValue(3, 3, 255);
    uBit.display.image.setPixelValue(4, 4, 255);
    fiber_sleep(75);
    uBit.display.image.setPixelValue(1, 3, 0);
    uBit.display.image.setPixelValue(3, 3, 0);
    uBit.display.image.setPixelValue(0, 3, 255);
    uBit.display.image.setPixelValue(1, 2, 255);
    uBit.display.image.setPixelValue(2, 2, 255);
    uBit.display.image.setPixelValue(3, 2, 255);
    uBit.display.image.setPixelValue(4, 3, 255);
    fiber_sleep(75);
    uBit.display.image.setPixelValue(1, 2, 0);
    uBit.display.image.setPixelValue(2, 3, 0);
    uBit.display.image.setPixelValue(3, 2, 0);
    uBit.display.image.setPixelValue(0, 1, 255);
    uBit.display.image.setPixelValue(0, 2, 255);
    uBit.display.image.setPixelValue(1, 1, 255);
    uBit.display.image.setPixelValue(3, 1, 255);
    uBit.display.image.setPixelValue(4, 1, 255);
    uBit.display.image.setPixelValue(4, 2, 255);
    fiber_sleep(75);
    uBit.display.image.setPixelValue(0, 0, 255);
    uBit.display.image.setPixelValue(4, 0, 255);
    playTone(0);
    fiber_sleep(500);
    showImage((char *)frame1);
    fiber_sleep(75);
    showImage((char *)frame2);
    fiber_sleep(75);
    showImage((char *)frame3);
    fiber_sleep(75);
    showImage((char *)endFrame);
    fiber_sleep(575);
    uBit.display.clear();
}

int main()
{
    uBit.init();

    uBit.serial.setBaud(115200);
    
    uBit.audio.levelSPL->setUnit(LEVEL_DETECTOR_SPL_8BIT);
    uBit.audio.activateMic();

    uBit.accelerometer.setRange(8);

    uart = new MicroBitUARTService(*uBit.ble, 200, 200);
    new MicroBitIOPinService(*uBit.ble, uBit.io);
    new MicroBitButtonService(*uBit.ble);
    new MicroBitLEDService(*uBit.ble, uBit.display);
    new MicroBitTemperatureService(*uBit.ble, uBit.thermometer);
    new MicroBitAccelerometerService(*uBit.ble, uBit.accelerometer);
    new MicroBitMagnetometerService(*uBit.ble, uBit.compass);

    uBit.bleManager.setTransmitPower(7);

    ON_LOGO(MICROBIT_BUTTON_EVT_CLICK);
    ON_LOGO(MICROBIT_BUTTON_EVT_DOWN);
    ON_LOGO(MICROBIT_BUTTON_EVT_UP);
    ON_LOGO(MICROBIT_BUTTON_EVT_LONG_CLICK);

    ON_GESTURE(MICROBIT_ACCELEROMETER_EVT_FREEFALL);
    ON_GESTURE(MICROBIT_ACCELEROMETER_EVT_SHAKE);
    ON_GESTURE(MICROBIT_ACCELEROMETER_EVT_TILT_DOWN);
    ON_GESTURE(MICROBIT_ACCELEROMETER_EVT_TILT_LEFT);
    ON_GESTURE(MICROBIT_ACCELEROMETER_EVT_TILT_RIGHT);
    ON_GESTURE(MICROBIT_ACCELEROMETER_EVT_TILT_UP);
    ON_GESTURE(MICROBIT_ACCELEROMETER_EVT_FACE_DOWN);
    ON_GESTURE(MICROBIT_ACCELEROMETER_EVT_FACE_UP);

    ON_LISTEN(MICROBIT_ID_IO_P0, MICROBIT_BUTTON_EVT_CLICK, P0Event);
    ON_LISTEN(MICROBIT_ID_IO_P0, MICROBIT_BUTTON_EVT_UP, P0Event);
    ON_LISTEN(MICROBIT_ID_IO_P1, MICROBIT_BUTTON_EVT_CLICK, P1Event);
    ON_LISTEN(MICROBIT_ID_IO_P1, MICROBIT_BUTTON_EVT_UP, P1Event);
    ON_LISTEN(MICROBIT_ID_IO_P2, MICROBIT_BUTTON_EVT_CLICK, P2Event);
    ON_LISTEN(MICROBIT_ID_IO_P2, MICROBIT_BUTTON_EVT_UP, P2Event);

    uBit.messageBus.listen(MICROBIT_ID_BLE, MICROBIT_BLE_EVT_CONNECTED, onBLEConnect,
                           MESSAGE_BUS_LISTENER_REENTRANT);

    uBit.messageBus.listen(MICROBIT_ID_BLE, MICROBIT_BLE_EVT_DISCONNECTED, onBLEDisconnect,
                           MESSAGE_BUS_LISTENER_REENTRANT);

    uBit.messageBus.listen(MICROBIT_ID_BLE_UART, MICROBIT_UART_S_EVT_DELIM_MATCH, onDelim,
                           MESSAGE_BUS_LISTENER_REENTRANT);

    char cmDelBuf[] = {CM_DEL};

    uart->eventOn(ManagedString((char *)cmDelBuf));

    genAnimationFrames();
    bootAnimation();

    uBit.audio.activateMic();
    
    release_fiber();
}